#include "pid.h"

int32_t pid_up(pid_t *pid,int32_t target,int32_t fb)
{
	pid->target = target;
	pid->fb = fb;
	pid->e = pid->target - pid->fb;
	pid->po_ = pid->e * pid->kp;
	pid->io_ += pid->e * pid->ki;
	pid->cout = pid->po_ + pid->io_ + pid->do_;
	pid->cout >>= 15;
	if(pid->cout >= pid->max_cout)
	{
		pid->cout = pid->max_cout;
	}
	else if(pid->cout <= -pid->max_cout)
	{
		pid->cout = -pid->max_cout;
	}
	return pid->cout;
}


